//
// Created by echo on 2019/11/26.
//

#ifndef PCD_COMPARE_GPSTOOLS_H
#define PCD_COMPARE_GPSTOOLS_H

#include <nav_msgs/Odometry.h>
#include <sensor_msgs/NavSatFix.h>
#include <boost/foreach.hpp>
#include <geometry_msgs/QuaternionStamped.h>
#include <Eigen/Core>

#define DEG_TO_RAD 0.01745329252
#define EARTH_MAJOR 6378137.0            ///< WGS84 MAJOR AXIS
#define EARTH_MINOR 6356752.31424518    ///< WGS84 MINOR AXIS

namespace common 
{
	class gpsTools 
	{
	public:
		gpsTools()
		{
			lla_origin_.setIdentity();
			for (size_t i = 0; i < 36; i++) gps_cur_cov_[i] = 0;
		};

		//1.ros msg 转 eigen
		static Eigen::Vector3d GpsMsg2Eigen(const sensor_msgs::NavSatFix &gps_msgs);
		//一般只用update GPSpose就行
		void updateGPSpose(const sensor_msgs::NavSatFix &gps_msgs);
		//2. LLA经度(longitude),纬度(latitude)和高度(altitude)经纬高坐标系 转(Earth-Centered, Earth-Fixed)
		// Z轴指向指向北，但不完全精确地与地球转动轴重合。转动轴有微小“摆动”，称之为“极运动(polar motion)”
		//X轴在球面上与格林威治线和赤道的交点
		Eigen::Vector3d LLA2ECEF(const Eigen::Vector3d &lla);
		Eigen::Vector3d ECEF2LLA(const Eigen::Vector3d &ecef);
		Eigen::Vector3d ECEF2ENU(const Eigen::Vector3d &ecef);
		Eigen::Vector3d ENU2ECEF(const Eigen::Vector3d &enu);
		//变量部分
		//1.lla的起点
		Eigen::Vector3d lla_origin_;
		//2.enu下的坐标
		Eigen::Vector3d gps_pos_;

		double gps_cur_cov_[36];

	private:
		static inline double deg2rad(const double &deg) 
		{
			return deg * DEG_TO_RAD;
		};
		static inline double rad2deg(const double &rad) 
		{
			return rad / DEG_TO_RAD;
		}
	};

	Eigen::Vector3d gpsTools::GpsMsg2Eigen(const sensor_msgs::NavSatFix &gps_msgs)
	{
		Eigen::Vector3d
			lla(gps_msgs.latitude, gps_msgs.longitude, gps_msgs.altitude);
		return lla;
	}

	Eigen::Vector3d gpsTools::LLA2ECEF(const Eigen::Vector3d &lla)
	{
		Eigen::Vector3d ecef;
		double lat = deg2rad(lla.x());
		double lon = deg2rad(lla.y());
		double alt = lla.z();
		double earth_r = pow(EARTH_MAJOR, 2) / sqrt(pow(EARTH_MAJOR * cos(lat), 2) + pow(EARTH_MINOR * sin(lat), 2));
		ecef.x() = (earth_r + alt) * cos(lat) * cos(lon);
		ecef.y() = (earth_r + alt) * cos(lat) * sin(lon);
		ecef.z() = (pow(EARTH_MINOR / EARTH_MAJOR, 2) * earth_r + alt) * sin(lat);

		return ecef;
	}

	Eigen::Vector3d gpsTools::ECEF2LLA(const Eigen::Vector3d &ecef)
	{
		double e =
			sqrt((pow(EARTH_MAJOR, 2) - pow(EARTH_MINOR, 2)) / pow(EARTH_MAJOR, 2));
		double e_ =
			sqrt((pow(EARTH_MAJOR, 2) - pow(EARTH_MINOR, 2)) / pow(EARTH_MINOR, 2));
		double p = sqrt(pow(ecef.x(), 2) + pow(ecef.y(), 2));
		double theta = atan2(ecef.z() * EARTH_MAJOR, p * EARTH_MINOR);

		double lon = atan2(ecef.y(), ecef.x());
		double lat = atan2((ecef.z() + pow(e_, 2) * EARTH_MINOR * pow(sin(theta), 3)),
						p - pow(e, 2) * EARTH_MAJOR * pow(cos(theta), 3));
		double earth_r = pow(EARTH_MAJOR, 2) / sqrt(pow(EARTH_MAJOR * cos(lat), 2) + pow(EARTH_MINOR * sin(lat), 2));
		double alt = p / cos(lat) - earth_r;
		Eigen::Vector3d lla(rad2deg(lat), rad2deg(lon), alt);
		return lla;
	}

	Eigen::Vector3d gpsTools::ECEF2ENU(const Eigen::Vector3d &ecef)
	{
		double lat = deg2rad(lla_origin_.x());
		double lon = deg2rad(lla_origin_.y());

		Eigen::Vector3d t = -LLA2ECEF(lla_origin_);
		Eigen::Matrix3d r;
		r << -sin(lon), cos(lon), 0,
		     -cos(lon) * sin(lat), -sin(lat) * sin(lon), cos(lat),
		     cos(lon) * cos(lat), sin(lon) * cos(lat), sin(lat);

		Eigen::Vector3d enu;
		enu = ecef + t;
		enu = r * enu;
		return enu;
	}

	Eigen::Vector3d gpsTools::ENU2ECEF(const Eigen::Vector3d &enu)
	{
		double lat = deg2rad(lla_origin_.x());
		double lon = deg2rad(lla_origin_.y());

		Eigen::Vector3d t = LLA2ECEF(lla_origin_);
		Eigen::Matrix3d r;
		r << -sin(lon), -cos(lon) * sin(lat), cos(lon) * cos(lat),
			cos(lon), -sin(lon) * sin(lat), sin(lon) * cos(lat),
			0, cos(lat), sin(lat);
		Eigen::Vector3d ecef;
		ecef = r * enu + t;
		return ecef;
	}

	void gpsTools::updateGPSpose(const sensor_msgs::NavSatFix &gps_msgs)
	{
		//检查状态4
		//因为zed的数据集有问题,所以包含状态0
		if (gps_msgs.status.status == 4 || gps_msgs.status.status == 5 || gps_msgs.status.status == 1 || gps_msgs.status.status == 2 || gps_msgs.status.status == 0)
		{
			//第一个的时候设置为起点
			if (lla_origin_ == Eigen::Vector3d::Identity())
			{
				Eigen::Vector3d lla = GpsMsg2Eigen(gps_msgs);
				lla_origin_ = lla;
				gps_pos_ = Eigen::Vector3d::Identity();
				//			std::cout<<"GPS origin: "<<lla_origin_<<"\n status: "<<gps_msgs.status.status<<std::endl;
			}
			else
			{
				Eigen::Vector3d lla = GpsMsg2Eigen(gps_msgs);
				Eigen::Vector3d ecef = LLA2ECEF(lla);
				Eigen::Vector3d enu = ECEF2ENU(ecef);
				gps_pos_ = enu;
				//			std::cout<<"GPS lla_origin_: "<<lla_origin_<<"\n curr"<<gps_pos_<<std::endl;
			}
			gps_cur_cov_[0] = gps_msgs.position_covariance[0];
			gps_cur_cov_[1] = gps_msgs.position_covariance[1];
			gps_cur_cov_[2] = gps_msgs.position_covariance[2];
			gps_cur_cov_[6] = gps_msgs.position_covariance[3];
			gps_cur_cov_[7] = gps_msgs.position_covariance[4];
			gps_cur_cov_[8] = gps_msgs.position_covariance[5];
			gps_cur_cov_[12] = gps_msgs.position_covariance[6];
			gps_cur_cov_[13] = gps_msgs.position_covariance[7];
			gps_cur_cov_[14] = gps_msgs.position_covariance[8];
		}
	}
}

#endif //PCD_COMPARE_GPSTOOLS_H
